10. Measurement Update

Mean Calculation

Measurement Update

<span class="mathquill ud-math">\mu</span> : Mean of the prior belief


<span class="mathquill ud-math">\sigma^{2}</span> : Variance of the prior belief



<span class="mathquill ud-math">v</span> : Mean of the measurement


<span class="mathquill ud-math">r^{2}</span> : Variance of the measurement

New Belief Quiz

Where do you think the robot's new belief will be?

SOLUTION: A

Measurement Update In 1D (Post Quiz)

The new mean is a weighted sum of the prior belief and measurement means. With uncertainty, a larger number represents a more uncertain probability distribution. However, the new mean should be biased towards the measurement update, which has a smaller standard deviation than the prior. How do we accomplish this?

\large \mu' = \frac{r^2 \mu + \sigma^2 v}{r^2 + \sigma^2}

The answer is - the uncertainty of the prior is multiplied by the mean of the measurement, to give it more weight, and similarly the uncertainty of the measurement is multiplied with the mean of the prior. Applying this formula to our example generates a new mean of 27.5, which we can label on our graph below.

Variance Calculation

Next, we need to determine the variance of the new state estimate.

What do you think? Would the variance of the new state estimate be less confident than our prior, in between our prior and measurement, or more confident than our measurement?

SOLUTION: More confident than our measurement.

The two Gaussians provide us with more information together than either Gaussian offered alone. As a result, our new state estimate is more confident than our prior belief and our measurement. This means that it has a higher peak and is narrower. You can see this in the graph below.

The formula for the new variance is presented below.

\large \sigma^{2'} = \frac{1}{\frac{1}{r^2} + \frac{1}{\sigma^2}}

Entering the variances from our example into this formula produces a new variance of 2.25. The new state estimate, often called the posterior, is drawn below.

<span class="mathquill ud-math">\mu</span> : Mean of the prior belief


<span class="mathquill ud-math">\sigma^{2}</span> : Variance of the prior belief



<span class="mathquill ud-math">v</span> : Mean of the measurement


<span class="mathquill ud-math">r^{2}</span> : Variance of the measurement



<span class="mathquill ud-math">\tau</span> : Mean of the posterior


<span class="mathquill ud-math">s^{2}</span> : Variance of the posterior

It’s time to implement these two formulas in C++. Place your code within a function called measurement_update, such that you can use it as a building block in your Kalman Filter implementation.

When you’re done, calculate the posterior mean and variance for a prior of N(x: μ_1=10, σ^2=8) and measurement N(x: μ_2=13, σ^2=2) . Is it what you expected?

Programming Quiz

In this C++ code, the measurement update function returns two values: the newly computed mean and variance. Usually, a tuple or struct should be used in C++ to return more than one value from a function and easily assign them later to multiple variables. For more information on tuples and structs take a look at this link .

Start Quiz: 

#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = //TODO: Code the measurment update mean function mu;
    new_var =  //TODO: Code the measurment update variance function sigma square;
    return make_tuple(new_mean, new_var);
}

int main()
{

    tie(new_mean, new_var) = measurement_update(10, 8, 13, 2);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}
#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}

int main()
{

    tie(new_mean, new_var) = measurement_update(10, 8, 13, 2);
    printf("[%f, %f]", new_mean, new_var);
    return 0;
}

I encourage you to think about what the posterior Gaussian would look like for the following example, and even calculate the exact values using your measurement_update function.

Next Concept